/*
    Dominate 3D - A cube-based game of life

    Copyright (C) 2007, Craig H. Miller (kidmosey@gmail.com)

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
////////////////////////////////////////
// CMatrix.cpp
////////////////////////////////////////

#include "matrix.h"

////////////////////////////////////////////////////////////////////////////////

CMatrix CMatrix::IDENTITY(1.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f,1.0f,0.0f,0.0f,0.0f);

////////////////////////////////////////////////////////////////////////////////

CMatrix::CMatrix(float ax,float ay,float az,float bx,float by,float bz,float cx,float cy,float cz,float dx,float dy,float dz) {
	a = CVector3D(ax,ay,az);
	b = CVector3D(bx,by,bz);
	c = CVector3D(cx,cy,cz);
	d = CVector3D(dx,dy,dz);

	pad0=pad1=pad2=0.0f;
	pad3=1.0f;
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::Identity() {
	a = CVector3D(1.0f,0.0f,0.0f);
	b = CVector3D(0.0f,1.0f,0.0f);
	c = CVector3D(0.0f,0.0f,1.0f);
	d = CVector3D(0.0f,0.0f,0.0f);
}

CMatrix CMatrix::operator* (const CMatrix &rhs) const
{
	CMatrix lhs;
	lhs.Dot(*this, rhs);

	return lhs;
}

CMatrix &CMatrix::operator*= (const CMatrix &rhs)
{
	Dot(*this, rhs);
	return *this;
}

////////////////////////////////////////////////////////////////////////////////
// Dot
////////////////////////////////////////////////////////////////////////////////
void CMatrix::Dot(const CMatrix &m,const CMatrix &n) {
	float ax=m.a.x*n.a.x+m.a.y*n.b.x+m.a.z*n.c.x;
	float ay=m.a.x*n.a.y+m.a.y*n.b.y+m.a.z*n.c.y;
	float az=m.a.x*n.a.z+m.a.y*n.b.z+m.a.z*n.c.z;

	float bx=m.b.x*n.a.x+m.b.y*n.b.x+m.b.z*n.c.x;
	float by=m.b.x*n.a.y+m.b.y*n.b.y+m.b.z*n.c.y;
	float bz=m.b.x*n.a.z+m.b.y*n.b.z+m.b.z*n.c.z;

	float cx=m.c.x*n.a.x+m.c.y*n.b.x+m.c.z*n.c.x;
	float cy=m.c.x*n.a.y+m.c.y*n.b.y+m.c.z*n.c.y;
	float cz=m.c.x*n.a.z+m.c.y*n.b.z+m.c.z*n.c.z;

	float dx=m.d.x*n.a.x+m.d.y*n.b.x+m.d.z*n.c.x+n.d.x;
	float dy=m.d.x*n.a.y+m.d.y*n.b.y+m.d.z*n.c.y+n.d.y;
	float dz=m.d.x*n.a.z+m.d.y*n.b.z+m.d.z*n.c.z+n.d.z;

	a = CVector3D(ax,ay,az);
	b = CVector3D(bx,by,bz);
	c = CVector3D(cx,cy,cz);
	d = CVector3D(dx,dy,dz);
}

////////////////////////////////////////////////////////////////////////////////
// Transform
////////////////////////////////////////////////////////////////////////////////

void CMatrix::Transform(const CVector3D &in,CVector3D &out) const {
	float x=in.x*a.x + in.y*b.x + in.z*c.x + d.x;
	float y=in.x*a.y + in.y*b.y + in.z*c.y + d.y;
	float z=in.x*a.z + in.y*b.z + in.z*c.z + d.z;
	out = CVector3D(x,y,z);
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::Transform3x3(const CVector3D &in,CVector3D &out) const {
	float x=in.x*a.x + in.y*b.x + in.z*c.x;
	float y=in.x*a.y + in.y*b.y + in.z*c.y;
	float z=in.x*a.z + in.y*b.z + in.z*c.z;
	out = CVector3D(x,y,z);
}

////////////////////////////////////////////////////////////////////////////////
// MakeRotate
////////////////////////////////////////////////////////////////////////////////

void CMatrix::MakeRotateX(float t) {
	float cost=cosf(t);
	float sint=sinf(t);

	a = CVector3D(1.0f, 0.0f, 0.0f);
	b = CVector3D(0.0f, cost, sint);
	c = CVector3D(0.0f,-sint, cost);
	d = CVector3D();
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::MakeRotateY(float t) {
	float cost=cosf(t);
	float sint=sinf(t);

	a = CVector3D(cost, 0.0f,-sint);
	b = CVector3D(0.0f, 1.0f, 0.0f);
	c = CVector3D(sint, 0.0f, cost);
	d = CVector3D();
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::MakeRotateZ(float t) {
	float cost=cosf(t);
	float sint=sinf(t);

	a = CVector3D( cost, sint, 0.0f);
	b = CVector3D(-sint, cost, 0.0f);
	c = CVector3D( 0.0f, 0.0f, 1.0f);
	d = CVector3D();
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::MakeRotateUnitAxis(const CVector3D &v,float t) {
	float cost=cosf(t);
	float sint=sinf(t);
	float omc=1.0f-cost;

	a.x=omc*v.x*v.x+cost;
	b.y=omc*v.y*v.y+cost;
	c.z=omc*v.z*v.z+cost;
	a.y=omc*v.x*v.y+sint*v.z;
	b.x=omc*v.x*v.y-sint*v.z;
	a.z=omc*v.x*v.z-sint*v.y;
	c.x=omc*v.x*v.z+sint*v.y;
	b.z=omc*v.y*v.z+sint*v.x;
	c.y=omc*v.y*v.z-sint*v.x;
	d = CVector3D();
}

////////////////////////////////////////////////////////////////////////////////
// Euler angles
////////////////////////////////////////////////////////////////////////////////

void CMatrix::FromEulers(const CVector3D &euler,int order) {
	float sx=sinf(euler.x),cx=cosf(euler.x);
	float sy=sinf(euler.y),cy=cosf(euler.y);
	float sz=sinf(euler.z),cz=cosf(euler.z);

	switch(order) {
		case EULER_XYZ:
			a = CVector3D(cy*cz,          cy*sz,         -sy);
			b = CVector3D(sx*sy*cz-cx*sz, sx*sy*sz+cx*cz, sx*cy);
			c = CVector3D(cx*sy*cz+sx*sz, cx*sy*sz-sx*cz, cx*cy);
			break;
		case EULER_XZY:
			a = CVector3D( cz*cy,           sz,    -cz*sy);
			b = CVector3D(-cx*sz*cy+sx*sy,  cx*cz,  cx*sz*sy+sx*cy);
			c = CVector3D( sx*sz*cy+cx*sy, -sx*cz, -sx*sz*sy+cx*cy);
			break;
		case EULER_YXZ:
			a = CVector3D( cy*cz-sy*sx*sz, cy*sz+sy*sx*cz, -sy*cx);
			b = CVector3D(-cx*sz,          cx*cz,           sx);
			c = CVector3D( sy*cz+cy*sx*sz, sy*sz-cy*sx*cz,  cy*cx);
			break;
		case EULER_YZX:
			a = CVector3D(cy*cz, cy*sz*cx+sy*sx, cy*sz*sx-sy*cx);
			b = CVector3D(  -sz, cz*cx,          cz*sx);
			c = CVector3D(sy*cz, sy*sz*cx-cy*sx, sy*sz*sx+cy*cx);
			break;
		case EULER_ZXY:
			a = CVector3D( cz*cy+sz*sx*sy, sz*cx, -cz*sy+sz*sx*cy);
			b = CVector3D(-sz*cy+cz*sx*sy, cz*cx,  sz*sy+cz*sx*cy);
			c = CVector3D( cx*sy,         -sx,     cx*cy);
			break;
		case EULER_ZYX:
			a = CVector3D( cz*cy, sz*cx+cz*sy*sx, sz*sx-cz*sy*cx);
			b = CVector3D(-sz*cy, cz*cx-sz*sy*sx, cz*sx+sz*sy*cx);
			c = CVector3D( sy,   -cy*sx,          cy*cx);
			break;
		default:
			//printf("ERROR: CMatrix::FromEulers()- Bad order specified\n");
			break;
	}

	d = CVector3D();
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::ToEulers(CVector3D &euler,int order) {
	switch(order) {
		case EULER_XYZ:
			euler = CVector3D(atan2f(b.z,c.z),asinf(-a.z),atan2f(a.y,a.x));
			break;
		case EULER_XZY:
			euler = CVector3D(atan2f(-c.y,b.y),atan2f(-a.z,a.x),asinf(a.y));
			break;
		case EULER_YXZ:
			euler = CVector3D(asinf(b.z),atan2f(-a.z,c.z),atan2f(-b.x,b.y));
			break;
		case EULER_YZX:
			euler = CVector3D(atan2f(b.z,b.y),atan2f(c.x,a.x),asinf(-b.x));
			break;
		case EULER_ZXY:
			euler = CVector3D(asinf(-c.y),atan2f(c.x,c.z),atan2f(a.y,b.y));
			break;
		case EULER_ZYX:
			euler = CVector3D(atan2f(-c.y,c.z),asinf(c.x),atan2f(-b.x,a.x));
			break;
		default:
			//printf("ERROR: CMatrix::ToEulers()- Bad order specified\n");
			break;
	}
}

////////////////////////////////////////////////////////////////////////////////
// Scale
////////////////////////////////////////////////////////////////////////////////

void CMatrix::MakeScale(float x,float y,float z) {
	a = CVector3D(x,0.0f,0.0f);
	b = CVector3D(0.0f,y,0.0f);
	c = CVector3D(0.0f,0.0f,z);
	d = CVector3D();
}

////////////////////////////////////////////////////////////////////////////////
// Inverse, Transpose
////////////////////////////////////////////////////////////////////////////////

inline float Det22(float ax,float ay,float bx,float by) {return ax*by-ay*bx;}

bool CMatrix::Inverse() {
	float subx, suby, subz;
	subx=Det22( b.y, b.z, c.y, c.z );
	suby=Det22( b.x, b.z, c.x, c.z );
	subz=Det22( b.x, b.y, c.x, c.y );

	float Det = a.x*subx - a.y*suby + a.z*subz; // Determinant
	if(Det==0.0f) {
		//printf("ERROR: CMatrix::Inverse()- Matrix not invertible\n");
		Identity();
		return false;
	}
	Det=1.0f/Det;		// Inverse of the determinant

	CMatrix Temp;
	Temp.a.x = subx*Det;
	Temp.b.x = -suby*Det;
	Temp.c.x = subz*Det;
	Temp.d.x = - (d.x*Temp.a.x + d.y*Temp.b.x + d.z*Temp.c.x);

	subx = Det22( a.y, a.z, c.y, c.z );
	suby = Det22( a.x, a.z, c.x, c.z );
	subz = Det22( a.x, a.y, c.x, c.y );

	Temp.a.y = -subx*Det;
	Temp.b.y = suby*Det;
	Temp.c.y = -subz*Det;
	Temp.d.y = - (d.x*Temp.a.y + d.y*Temp.b.y + d.z*Temp.c.y);

	subx = Det22( a.y, a.z, b.y, b.z );
	suby = Det22( a.x, a.z, b.x, b.z );
	subz = Det22( a.x, a.y, b.x, b.y );

	Temp.a.z = subx*Det;
	Temp.b.z = -suby*Det;
	Temp.c.z = subz*Det;
	Temp.d.z = - (d.x*Temp.a.z + d.y*Temp.b.z + d.z*Temp.c.z);

	(*this)=Temp;
	return true;
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::FastInverse() {
	CMatrix tmp;

	tmp.a = CVector3D(a.x,b.x,c.x);
	tmp.b = CVector3D(a.y,b.y,c.y);
	tmp.c = CVector3D(a.z,b.z,c.z);
	tmp.d.x = -(d.dot(a));
	tmp.d.y = -(d.dot(b));
	tmp.d.z = -(d.dot(c));

	(*this)=tmp;
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::Transpose() {
	float t;
	t=b.x; b.x=a.y; a.y=t;
	t=c.x; c.x=a.z; a.z=t;
	t=c.y; c.y=b.z; b.z=t;
}

////////////////////////////////////////////////////////////////////////////////
// Viewing
////////////////////////////////////////////////////////////////////////////////

void CMatrix::LookAt(const CVector3D &from,const CVector3D &to)
{
	d = from;
	c = d - to;
	c.normalize();

	a = CVector3D(c.z,0.0f,-c.x);
	a.normalize();			// BUG: this can blow up

	b = c.cross(a);
}

////////////////////////////////////////////////////////////////////////////////

void CMatrix::PolarView(float dist,float azm,float inc,float twst)
{
	CVector3D v(-inc,azm,twst);
	FromEulers(v,EULER_ZXY);
	d = c * dist;
}

////////////////////////////////////////////////////////////////////////////////
// Misc functions
////////////////////////////////////////////////////////////////////////////////

float CMatrix::Determinant3x3() const
{
	return a.x*b.y*c.z +
		   a.y*b.z*c.x +
		   a.z*b.x*c.y -
		   a.x*b.z*c.y -
		   a.y*b.x*c.z -
		   a.z*b.y*c.x;
}
